#include "rclcpp/rclcpp.hpp"
#include <Eigen/Core>

double distanceTolerance = 0.1, angleTolerance = 0.1;

// 判断机器人是否到达目标点
bool isAtGoal(const Eigen::VectorXd &currentPose, const Eigen::VectorXd &goalPose)
{
    // 计算位置差和姿态差的绝对值
    Eigen::VectorXd positionDiff = currentPose.head(3) - goalPose.head(3);
    Eigen::VectorXd orientationDiff = currentPose.tail(3) - goalPose.tail(3);
    positionDiff = positionDiff.cwiseAbs();
    orientationDiff = orientationDiff.cwiseAbs();

    // 判断是否到达目标点
    bool isPositionAtGoal = (positionDiff.norm() < distanceTolerance);
    bool isOrientationAtGoal = (orientationDiff.norm() < angleTolerance);
    return (isPositionAtGoal && isOrientationAtGoal);
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    Eigen::VectorXd currentPose(6);
    currentPose << 1.168, 22.791, 75.479, -0.399, 80.661, 2.005;
    Eigen::VectorXd goalPose(6);
    goalPose << 1.168, 22.791, 75.479, -0.4, 80.671, 177.9;
    if (isAtGoal(currentPose, goalPose))
    {
        std::cout << "is at goal..." << std::endl;
    }
    else
    {
        std::cout << "not at goal..." << std::endl;
    }
    return 0;
}